#include "kernel_functions.h"

#include <string>
#include <vector>
#include <iostream>
#include <fstream>
#include <stdint.h>
#include <sstream>

#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>



/**
 * @brief 加载图像
 * @param[in]  strPathLeft          保存左目图像文件名称的文件的路径
 * @param[in]  strPathRight         保存右目图像文件名称的文件的路径
 * @param[in]  strPathTimes         保存图像时间戳的文件的路径
 * @param[out] vstrImageLeft        左目图像序列中每张图像的文件名
 * @param[out] vstrImageRight       右目图像序列中每张图像的文件名
 * @param[out] vTimeStamps          图像序列中每张图像的时间戳(认为左目图像和右目图像的时间戳已经对齐)
 */
int32_t step_load_images(const string &strPathLeft, const string &strPathRight, const string &strPathTimes,
                vector<string> &vstrImageLeft, vector<string> &vstrImageRight, vector<double> &vTimeStamps)
{
    ifstream fTimes;
    fTimes.open(strPathTimes.c_str());
    vTimeStamps.reserve(5000);
    vstrImageLeft.reserve(5000);
    vstrImageRight.reserve(5000);
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            vstrImageLeft.push_back(strPathLeft + "/" + ss.str() + ".png");
            vstrImageRight.push_back(strPathRight + "/" + ss.str() + ".png");
            double t;
            ss >> t;
            vTimeStamps.push_back(t/1e9);
        }
    }

        //检查图像序列是否为空
    if(vstrImageLeft.empty() || vstrImageRight.empty() || vTimeStamps.empty())
    {
        cerr << "ERROR: No images in provided path." << endl;
        return -1;
    }

    // 当然如果左右目图像的数目不一致也不可以
    if(vstrImageLeft.size()!=vstrImageRight.size() || vstrImageLeft.size()!=vTimeStamps.size())
    {
        cerr << "ERROR: Different number of left and right images." << endl;
        return -2;
    }

    return 0;
}

int32_t step_read_configs(const string &strPathConfig, cv::Mat &M1l, cv::Mat &M2l, cv::Mat &M1r,cv::Mat &M2r)
{
    cv::FileStorage fsSettings(
            strPathConfig,                    // path_to_settings
            cv::FileStorage::READ);     // 以只读方式打开
    if(!fsSettings.isOpened())
    {
        cerr << "ERROR: Wrong path to settings" << endl;
        return -1;
    }
    cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
    //相机内参
    fsSettings["LEFT.K"] >> K_l;
    fsSettings["RIGHT.K"] >> K_r;
    //TODO 目测是经过双目立体矫正后的投影矩阵
    fsSettings["LEFT.P"] >> P_l;
    fsSettings["RIGHT.P"] >> P_r;

    // 修正变换矩阵,见下面的函数调用
    fsSettings["LEFT.R"] >> R_l;
    fsSettings["RIGHT.R"] >> R_r;

    //去畸变参数
    fsSettings["LEFT.D"] >> D_l;
    fsSettings["RIGHT.D"] >> D_r;

    //图像尺寸
    int rows_l = fsSettings["LEFT.height"];
    int cols_l = fsSettings["LEFT.width"];
    int rows_r = fsSettings["RIGHT.height"];
    int cols_r = fsSettings["RIGHT.width"];

    //参数合法性检查(不为空就行)
    if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
            rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
    {
        cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
        return -1;
    }

    //根据相机内参,去畸变参数以及立体矫正后,得到的新的相机内参来计算从原始图像到处理后理想双目图像的映射矩阵
    //这个函数的定义参考:[https://blog.csdn.net/u013341645/article/details/78710740]
    // 左目
    cv::initUndistortRectifyMap(            //计算无畸变和修正转换映射
        K_l,                                //输入的相机内参矩阵 (单目标定阶段得到的相机内参矩阵)
        D_l,                                //单目标定阶段得到的相机的去畸变参数
        R_l,                                //可选的修正变换矩阵,3*3, 从 cv::stereoRectify 得来.如果这个矩阵为空矩阵,那么就将会被设置成为单位矩阵
        P_l.rowRange(0,3).colRange(0,3),    //新的相机内参矩阵
        cv::Size(cols_l,rows_l),            //在去畸变之前的图像尺寸
        CV_32F,                             //第一个输出映射的类型
        M1l,                                //第一个输出映射表
        M2l);                               //第二个输出映射
    // 右目
    cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);

    return 0;
}

int32_t step_calibrate_stereo_imgs(
    const cv::String &src_left_filename, const cv::String &src_right_filename, // 输入 原始左右图像的文件名
    const cv::InputArray src_left, const cv::InputArray src_right, // 输入 原始左右图像
    cv::OutputArray &dst_left, cv::OutputArray &dst_right,  // 输出 校正后的图像
    const cv::InputArray map1, const cv::InputArray map2, const int interpolation)
{
        //合法性检查
        // cout << src_left_filename << endl;
        if(src_left.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << src_left_filename << endl;
            return -1;
        }

        // cout << src_right_filename << endl;
        if(src_right.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << src_right_filename << endl;
            return -2;
        }

        // step 4.2 对左右目图像进行双目矫正和去畸变处理
        //参考博客 [https://blog.csdn.net/sss_369/article/details/52983123]
        // 左目
        cv::remap(              //重映射，就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。
            src_left,             //输入图像
            dst_left,         //输出图像
            map1,                //第一个映射矩阵表
            map2,                //第二个映射矩阵
            cv::INTER_LINEAR);
            
        // 右目
        cv::remap(src_right,dst_right,map1,map2,cv::INTER_LINEAR);

        return 0;
}